/* Blimpbots

   In-flight control code, for Blimpbot 1

   Jeff Powers
   Sam Wintermute
   Jason Dietrich

   January-April 2007

*/


#include <p18f1330.h>
#include <usart.h> /* library custom compiled by jeff */
#include <adc.h>  /* library custom compiled by jeff */
#include "sw_i2c.h"
#include <stdlib.h>
#include <delays.h>

/* Watchdog timer = OFF */
#pragma config WDT = OFF 

void int_handler (void);
unsigned int readCompass2(void);
void motorsOff(void);


#define BUF_SIZE 25

#pragma code high_int = 0x8
void at_high_int_handler (void)
{
  _asm goto int_handler _endasm
}
#pragma code

int motor_disable = 0;

int adc;
int adc2;
int val_h;
int val_l;

int lv_count;
int lv_count_max = 15;

char val_str[7];

char dc1Next;
char dc2Next;
char dc3Next;
char i2cMode;

void printVal(char value)
{
    char val[7];
    btoa(value, val);
	putsUSART (val);
	putrsUSART ((const far rom char *)" ");
}

unsigned int readCompass2(void)
{
   unsigned int compassValue;

   char I2C_out[1] = {'A'};
   unsigned char I2C_in[2];

   I2C_Send(0x42, I2C_out,1);

   Delay10KTCYx(5); // wait for capture and calibration

   I2C_Read(0x43, (char*)I2C_in,2);

   compassValue = I2C_in[1] + (I2C_in[0]*256);

   return compassValue;
}


void wakeCompass2(void)
{
    char I2CBUFF[1]={'A'};
    I2C_Send(0x42, I2CBUFF,1);
}

void setOperationalMode(void)
{
    char I2CBUFF[3]={'G',0x74, 0b00100001};

    ADCON1bits.PCFG0 = 1; /* Analog port 0 off -- RA0 I/O on */
    ADCON1bits.PCFG3 = 1; /* Analog port 3 off -- RA6 I/O on */

    I2C_Send(0x42, I2CBUFF,3);
}

void getValue (void)
{
    int i;
    char val[7];

    ADCON0bits.ADON = 1; // Turn ADC on
    ADCON0bits.GO = 1;

	while(ADCON0bits.NOT_DONE); //wait for result

	adc = ReadADC(); //get ADC result

	putrsUSART ((const far rom char *)"\n\r(V=");
    itoa(adc, val);
	putsUSART (val);
	putrsUSART ((const far rom char *)")\n\r");

    ADCON0bits.ADON = 0; // Turn off ADC

}


void printCompass (void)
{
    unsigned int heading;
    char val[7];

	heading = readCompass2();

	putrsUSART ((const far rom char *)"\n\r Heading = ");
    itoa(heading, val);
	putsUSART (val);
	putrsUSART ((const far rom char *)"\n\r");

}


unsigned int getDC(char c) {
  // read in a duty cycle
  // take an integer, convert it to a % of the max DC value (2^12-1)
  // 1-9 are 10%-90%, 0 is 100%, and ` is 0% (following the top of the
  // keyboard)

  if (c == '`') {
    return 0;
  }
  else if (c == '-') {
	return 204;
  }
  else if (c == '1') {
    return 409;
  }
  else if (c == '2') {
    return 819;
  }
  else if (c == '3') {
    return 1229;
  }
  else if (c == '4') {
    return 1638;
  }
  else if (c == '5') {
    return 2048;
  }
  else if (c == '6') {
    return 2458;
  }
  else if (c == '7') {
    return 2867;
  }
  else if (c == '8') {
    return 3277;
  }
  else if (c == '9') {
    return 3686;
  }
  else if (c == '0') {
    return 4095;
  } 

  return 4096; // error code
}

void motorsOff(void) {
  PDC0L = (unsigned short) 0;
  PDC0H = (unsigned short) 0;
  PDC1L = (unsigned short) 0;
  PDC1H = (unsigned short) 0;
  PDC2L = (unsigned short) 0;
  PDC2H = (unsigned short) 0;
  return;
}

#pragma interrupt int_handler
void int_handler (void)
{
  unsigned char c;
  char I2CBUFF[1];
  char val[7];
  int process_key;


  if (PIR2bits.LVDIF == 1) {
    // low voltage detected!
      lv_count++;
      if (lv_count >= lv_count_max) {

        putrsUSART ((const far rom char *)" Voltage is too low! Shutting down.\n\r");
        motorsOff();
		motor_disable = 1;
      }
      else {
        putrsUSART((const far rom char *)"LV");

        itoa(lv_count_max - lv_count, val);
	    putsUSART (val);
        PIR2bits.LVDIF = 0;

        return;
      }
  }
  else {
    // assume its a USART interrupt
    if (PIR1bits.RCIF == 0) {
        putrsUSART ((const far rom char *)" ERROR: unknown interrupt!\n\r");
        motorsOff();
		motor_disable = 1;
    }

    /* Turn status LED off to indicate processing */
    PORTBbits.RB3 = 0; 

    /* Get the character received from the USART */
    c = ReadUSART();
    putcUSART(c); // echo it back 

    process_key = 1;

    if (i2cMode) {

      process_key = 0;

      i2cMode = 0;
      I2CBUFF[0] = c;
      if(I2C_Send(0x42, I2CBUFF,1))
        putrsUSART ((const far rom char *)" Success!\n\r");
      else
        putrsUSART ((const far rom char *)" Fail.\n\r");
    }

    if(motor_disable == 0)
    {
        process_key = 0;

	    if (dc1Next) { 
	      unsigned int dc = getDC(c);
	      if (dc >> 12) {
	        //error
	      }
	      else {
	        PDC0L = (unsigned short) ((dc << 2) & 0xFF); // lower 6 bits go here, skipping first 2 bits
	        PDC0H = (unsigned short) ((dc >> 6) & 0x3F); // upper 6 bits here, two highest are reserved.
	      }
	      dc1Next = 0;
	    }
	    else if (dc2Next) { 
	      unsigned int dc = getDC(c);
	      if (dc >> 12) {
	        //error
	      }
	      else {
	        PDC1L = (unsigned short) ((dc << 2) & 0xFF); // lower 6 bits go here, skipping first 2 bits
	        PDC1H = (unsigned short) ((dc >> 6) & 0x3F); // upper 6 bits here, two highest are reserved.
	      }
	      dc2Next = 0;
	    }
	    else if (dc3Next) { 
	      unsigned int dc = getDC(c);
	      if (dc >> 12) {
	        //error
	      }
	      else {
	        PDC2L = (unsigned short) ((dc << 2) & 0xFF); // lower 6 bits go here, skipping first 2 bits
	        PDC2H = (unsigned short) ((dc >> 6) & 0x3F); // upper 6 bits here, two highest are reserved.
	      }
	      dc3Next = 0;
	    }
        else process_key = 1; 
    }

    if(process_key == 1)
    {
	    if(c == 'v')
	    {
	      getValue();
	    }
	    else if(c == 'h')
	    {
	      printCompass();
	    }
	    else if(c == 'i')
	    {
	      i2cMode = 1;
  	      putrsUSART ((const far rom char *)"\n\rI2C Command: ");
	    }

	    // LED control
	    else if (c == '[') { PORTBbits.RB2 = 0; } // COLOR 1
	    else if (c == ']') { PORTBbits.RB2 = 1; } // COLOR 2
	    else if (c == '{') { PORTAbits.RA7 = 0; } // LED OFF 
	    else if (c == '}') { PORTAbits.RA7 = 1; } // LED ON 
	    
	    else if (c == 'r') { motor_disable = 0; lv_count = 0; } // re-enable motors
	
	   // pwm 1 control
		else if (c == 'q') {
	      dc1Next = 1;
	    }
	    else if (c == 'a') {
	    PORTBbits.RB0 = 1;
	    }
	    else if (c == 'z') {
	    PORTBbits.RB0 = 0;
	    }
	    // pwm 2 control
	    else if (c == 'w') {
	      dc2Next = 1;
	    }
	    else if (c == 's') {
	    PORTBbits.RB4 = 1;
	    }
	    else if (c == 'x') {
	    PORTBbits.RB4 = 0;
	    }
	    // pwm 3 control
	    else if (c == 'e') {
	      dc3Next = 1;
	    }
	    else if (c == 'd') {
	    PORTBbits.RB6 = 1;
	    }
	    else if (c == 'c') {
	    PORTBbits.RB6 = 0;
	    }

	} // end if process key

    /* Clear the interrupt flag */
    PIR1bits.RCIF = 0;

    /* Return status LED to default (on) state */
    PORTBbits.RB3 = 1; 
  }

}



void main (void)
{
  char val[7];
  int p;

  lv_count = 0;

  // Compass I2C Interface
  // RA6 = SCL
  // RA0 = SDA

  dc1Next = 0;
  dc2Next = 0;
  dc3Next = 0;

  i2cMode = 0;

  /* Set frequency to 8 MHz */
  OSCCONbits.IRCF2 = 1;
  OSCCONbits.IRCF1 = 1;
  OSCCONbits.IRCF0 = 1;

  /* Wait until frequency stable */
  while(!OSCCONbits.FLTS) ;

  /* Configure all PORTB pins for output */
  TRISB = 0x00;

  /* Configure some PORTA pins as input */
  TRISAbits.TRISA0 = 1; //RA0 = input
  TRISAbits.TRISA1 = 1; //RA1 = input
  TRISAbits.TRISA3 = 1; //RA3 = input
  TRISAbits.TRISA4 = 1; //RA4 = input

  TRISAbits.TRISA7 = 0; //RA7 = output (LED on/off)
  TRISAbits.TRISA6 = 0; //RA6 = output

  T0CON = 0x00; /* Shut off timer 0 */

  /*PORTA = 0b11000000; /* Set RA6,RA7 */

  PORTA = 0x00;
  LATA = 0x00;

  /* Turn status LED on */
  PORTBbits.RB3 = 1; 

  // initialize pwm
  PWMCON0 = 0b01110111; // Odd inputs PWM.  Independent mode.
  PWMCON1 = 0b11110000;
  PTCON0 = 0b00000000; // 1:1 prescale (formerly 1:16)
  PTCON1 = 0b10000000;

  PDC0L =  0b00000000;
  PDC0H =  0b00000000;
  PDC1L =  0b00000000;
  PDC1H =  0b00000000;
  PDC2L =  0b00000000;
  PDC2H =  0b00000000;

   /*
   * Open the USART configured as
   * 8N1, 9600 baud, in polled mode
   */
  OpenUSART (USART_TX_INT_OFF &
             USART_RX_INT_ON &
             USART_ASYNCH_MODE &
             USART_EIGHT_BIT &
             USART_CONT_RX &
             USART_BRGH_LOW, 12);

  p = (int)&ADRESL;
  itoa(p, val);
  putsUSART (val);
  putcUSART('!');

  /* Enable interrupt priority */
  RCONbits.IPEN = 1;

  /* Make receive interrupt high priority */
  IPR1bits.RCIP = 1;

  /* Enable all high priority interrupts */
  INTCONbits.GIEH = 1;
  INTCONbits.RBIE = 0;

  PIE1bits.ADIE = 0;
  PIE1bits.RCIE = 1; /* EUSART RX interrupt enable */
  PIE1bits.CMP0IE = 0;
  PIE1bits.CMP1IE = 0;
  PIE1bits.CMP2IE = 0;
  PIE1bits.TMR1IE = 0;

  /* Disable the comparator */
  /* CMCONbits.CMEN2 = 0;
  CMCONbits.CMEN1 = 0;
  CMCONbits.CMEN0 = 0; */

  ADCON0bits.ADON = 0; // Turn off ADC

  /* Initialization for the I2C Compass & Voltage Readout */
  
  ADCON1bits.PCFG0 = 1; /* 1 => AN0 off -- RA0 I/O on */
  //ADCON1bits.PCFG1 = 1; /* 1 => AN1 off -- RA1 I/O on */
  ADCON1bits.PCFG2 = 0; /* 0 => AN2 on -- RA4 I/O off */
  //ADCON1bits.PCFG3 = 1; /* 1 => AN3 off -- RA6 I/O on */
  ADCON1bits.VCFG0 = 0; /* Vref == Vdd */

  ADCON2 = 0b10010000;  // Right-justify, 4 TAD, TOSC=FOSC/2
  ADCON0 = 0b00001000; //  SEVTEN=0, AD CH = 2, still off

  /* init the low-voltage detect */
  // p. 183 of spec
  // turn off before setting values
  LVDCONbits.LVDEN = 0;
  // LVDL settings: min, typ, max
  // 0101 2.65 2.79 2.93
  // 0110 2.74 2.89 3.04
  // 0111 2.96 3.12 3.28
  LVDCONbits.LVDL3 = 0;
  LVDCONbits.LVDL2 = 1;
  LVDCONbits.LVDL1 = 0;
  LVDCONbits.LVDL0 = 1;

  // high priority
  IPR2bits.LVDIP = 1;
  // turn on
  LVDCONbits.LVDEN = 1;
  // clear present in case set
  PIR2bits.LVDIF = 0;
  // enable
  PIE2bits.LVDIE = 1;


  /* Loop forever */
  while (1)
    ;
}
